#include "testMotorJoystick.h"
#include "arduinomega/HardwareSerial.h"

#define DEBUG true

extern "C" void __cxa_pure_virtual(){}

void setup();
void loop();
void printMotorInput();


int main()
{
	// The init() function is defined in the Arduino library.
	// It initialises the timers and the ADC on the ATmega1280.

	init();

	JoystickMotor joystickMotor;

	joystickMotor.setup();

	while( true )
	{
		joystickMotor.loop();
		delay(5000);
		//joystickMotor.printMotorInput();
	}

	return 0;
}

void JoystickMotor::setup()
{
	//set baud rate for serial output
	Serial.begin( BAUD_RATE );

	//set pins to receive input
	pinMode(ENABLE_RIGHT_MOTOR_PIN, OUTPUT);
	pinMode(ENABLE_LEFT_MOTOR_PIN, OUTPUT);

	pinMode( SERVO_JOYSTICK_POWER_PIN, OUTPUT );
	pinMode( RIGHT_MOTOR_CONTROL_PIN_1, OUTPUT);
	pinMode( RIGHT_MOTOR_CONTROL_PIN_2, OUTPUT);
}

void JoystickMotor::loop()
{

	rightPotentiometerReading = analogRead( RIGHT_MOTOR_PIN );
	leftPotentiometerReading = analogRead( LEFT_MOTOR_PIN );
	servoPotentiometerReading = analogRead( SERVO_PIN );

	printMotorInput( );

	turn_magnitude = leftPotentiometerReading - rightPotentiometerReading;

	uint8_t r_power;
	uint8_t l_power;

	//setup duty cycle (three speed motor)
	if( rightPotentiometerReading > 800 || rightPotentiometerReading < 200 )
	{
		r_power = FULL_DUTY;
	}
	else if( rightPotentiometerReading > 600 || rightPotentiometerReading < 400 )
	{
		r_power = HALF_DUTY;
	}
	else
	{
		r_power = OFF_DUTY;
	}

	//setup duty cycle (three speed motor)
	if( leftPotentiometerReading > 800 || leftPotentiometerReading < 200 )
	{
		l_power = FULL_DUTY;
	}
	else if( leftPotentiometerReading > 600 || leftPotentiometerReading < 400 )
	{
		l_power = HALF_DUTY;
	}
	else
	{
		l_power = OFF_DUTY;
	}

	//set direction
	if( rightPotentiometerReading > 512 )
	{
		//turn one low to make sure you don't break;
		digitalWrite( RIGHT_MOTOR_CONTROL_PIN_2, LOW );
		digitalWrite( RIGHT_MOTOR_CONTROL_PIN_1, HIGH );
	}
	else
	{
		//turn one low to make sure you don't break;
		digitalWrite( RIGHT_MOTOR_CONTROL_PIN_1, LOW );
		digitalWrite( RIGHT_MOTOR_CONTROL_PIN_2, HIGH );
	}

	if( leftPotentiometerReading > 512 )
	{
		//turn one low to make sure you don't break;
		digitalWrite( LEFT_MOTOR_CONTROL_PIN_2, LOW );
		digitalWrite( LEFT_MOTOR_CONTROL_PIN_1, HIGH );
	}
	else
	{
		//turn one low to make sure you don't break;
		digitalWrite( LEFT_MOTOR_CONTROL_PIN_1, LOW );
		digitalWrite( LEFT_MOTOR_CONTROL_PIN_2, HIGH );
	}

	//set duty cycle
	analogWrite( ENABLE_RIGHT_MOTOR_PIN, r_power);
	analogWrite( ENABLE_LEFT_MOTOR_PIN, l_power);
}

void JoystickMotor::printMotorInput()
{
	//Serial.print( "r: " );
	//Serial.print( rightPotentiometerReading );
	//Serial.print("\n\r");
	//Serial.print( "l: " );
	//Serial.print( leftPotentiometerReading );
	//Serial.print("\n\r");
	Serial.print( "s: " );
	Serial.print( servoPotentiometerReading  );
	Serial.print("\n\r");
	delay( 200 );
}
